
import rospy
import numpy as np
import tf.transfrmations as tfs

from arm_manager import ArmManager
from gripper_manager import GripperManager
from chassis import ChassisManager
from sensors_init import SensorsInit

from std_msgs.msg import UInt8
from std_msgs.msg import Float64MultiArray
from webots_ros.msg import RecognitionObject

enable_flag = -1
objects_info = ['can','model']

def get_sign_callback(msg_sign):
    enable_flag = msg_sign.data

def get_target_pose_callback(msg_joint_value):
    global cmarm
    cmarm.set_velocity(msg_joint_value.data,0.13)
    rospy.sleep(5)
    cmgripper.set_velocity(0.5,0.15)
    rospy.sleep(5)
    cmgripper.set_velocity(0,0.15)

def get_object_info_callback(msg_object_info):
    if(enable_flag==objects_info.index(msg_object_info.model)):
        ob_to_camera = np.array[[msg_object_info.position.x,msg_object_info.position.y,msg_object_info.position.z,1]]
        camera_to_base_T = np.array[[]]
        camera_to_base_R = tfs.euler_matrix()
        ob_to_base = np.dot(camera_to_base_R,ob_to_camera.T)+camera_to_base_T.T


    # msg_object_info.position



    # msg_object_info.model
    # msg_object_info.position
    pass

def run():
    #arm
    cmarm = ArmManager()
    #gripper
    cmgripper = GripperManager()
    #chassis
    cm = ChassisManager()
    cm.run()
    #camera
    si = SensorsInit()
    si.enable_sensors()
    #get objects info
    rospy.Subscriber('/joint_group_position_controller/command', Float64MultiArray, get_target_pose_callback)
    rospy.Subscriber('/robot/kinect_color/recognition_objects', RecognitionObject, get_object_info_callback)
    rospy.spin()

if __name__ == "__main__":
    rospy.init_node('main_node', anonymous= False)
    run()

# rospy.Subscriber('/sign', UInt8, get_sign_callba
# armp1 = [0,0,0,0,0]
# armp2 = [0.0,0.98,0,0.61,0]
# armp3 = [0.22,0.98,0,0.61,0]
# armp4 = [0.42,0.98,0,0.61,0]
# speed = 0.13
# cmarm.set_velocity(armp2,speed)
# rospy.sleep(5)
# cmarm.set_velocity(armp4,speed)
# rospy.sleep(5)

# cmgripper.set_velocity(0.5,0.15)
# rospy.sleep(5)

# cmarm.set_velocity(armp3,speed)
# # rospy.sleep(5)

# header: 
#   seq: 4563
#   stamp: 
#     secs: 1640335650
#     nsecs: 213010582
#   frame_id: "robot/kinect_color"
# position: 
#   x: -0.031323908948127824
#   y: -0.024099580180420122
#   z: -0.6035435351282126
# orientation: 
#   x: -0.09619654053159829
#   y: 0.7499328579314014
#   z: -0.08164435748248483
#   w: 0.6493697968614381
# position_on_image: 
#   x: 574.0
#   y: 410.0
#   z: 0.0
# size_on_image: 
#   x: 135.0
#   y: 282.0
#   z: 0.0
# number_of_colors: 1
# colors: 
#   - 
#     x: 0.666667
#     y: 0.0
#     z: 0.0
# model: "can"
